{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "7c3db737",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 9: Dynamics and Control"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f46d8ac5",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    from google.colab import output\n",
    "    print('Running on CoLab')\n",
    "    output.enable_custom_widget_manager()\n",
    "    !pip install ipympl\n",
    "    !pip install roboticstoolbox-python>=1.0.2\n",
    "    !pip install --no-deps rvc3python\n",
    "    COLAB = True\n",
    "\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# add RTB examples folder to the path\n",
    "import sys, os.path\n",
    "import RVC3 as rvc\n",
    "sys.path.append(os.path.join(rvc.__path__[0], 'models'))\n",
    "\n",
    "# helper function to run bdsim in a subprocess and transfer results using a pickle file\n",
    "import pickle\n",
    "def run_shell(tool, **params):\n",
    "    global out\n",
    "    pyfile = os.path.join(rvc.__path__[0], \"models\", tool+\".py\")\n",
    "    cmd = f\"python {pyfile} -H +a -o\"\n",
    "    for key, value in params.items():\n",
    "        cmd += f' --global \"{key}={value}\"'\n",
    "    print(cmd)\n",
    "    os.system(cmd)\n",
    "    with open(\"bd.out\", \"rb\") as f:\n",
    "        out = pickle.load(f)\n",
    "\n",
    "# ------ standard imports ------ #\n",
    "import numpy as np\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *\n",
    "from roboticstoolbox import *"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "36c17bea",
   "metadata": {},
   "source": [
    "# 9.1 Independent Joint Control\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "efb31a21",
   "metadata": {},
   "source": [
    "## 9.1.1 Actuators\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14bcc27b",
   "metadata": {},
   "source": [
    "## 9.1.2 Friction\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e571957f",
   "metadata": {},
   "source": [
    "## 9.1.3 Effect of the Link Mass\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0af7ed1b",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy\n",
    "a1, a2, r1, r2, m1, m2, g = sympy.symbols(\"a1 a2 r1 r2 m1 m2 g\")\n",
    "link1 = Link(ET.Ry(flip=True), m=m1, r=[r1, 0, 0], name=\"link0\")\n",
    "link2 = Link(ET.tx(a1) * ET.Ry(flip=True), m=m2, r=[r2, 0, 0], name=\"link1\")\n",
    "robot = ERobot([link1, link2])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77e77b06",
   "metadata": {},
   "outputs": [],
   "source": [
    "robot.dynamics()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "909d24a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = sympy.symbols(\"q:2\")\n",
    "qd = sympy.symbols(\"qd:2\")\n",
    "qdd = sympy.symbols(\"qdd:2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e255fa2",
   "metadata": {},
   "outputs": [],
   "source": [
    "tau = robot.rne(q, qd, qdd, gravity=[0, 0, g], symbolic=True);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd51b002",
   "metadata": {},
   "source": [
    "## 9.1.4 Gearbox\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "dd6f961f",
   "metadata": {},
   "source": [
    "## 9.1.5 Modeling the Robot Joint\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fe511a98",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();  # load model of PUMA560 with dynamic parameters\n",
    "tf = puma.jointdynamics(puma.qn);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bf6e3009",
   "metadata": {},
   "outputs": [],
   "source": [
    "tf[1]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0f760466",
   "metadata": {},
   "source": [
    "## 9.1.6 Velocity Control Loop\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bcba27ce",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m vloop_test -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"vloop_test\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a97f4eb4",
   "metadata": {},
   "source": [
    "## 9.1.7 Position Control Loop\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c2004e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m ploop_test -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"ploop_test\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e6193ca0",
   "metadata": {},
   "source": [
    "## 9.1.8 Independent Joint Control Summary\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2e6f06e8",
   "metadata": {},
   "source": [
    "# 9.2 Rigid-Body Equations of Motion\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c36044cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9af9ebd2",
   "metadata": {},
   "outputs": [],
   "source": [
    "zero = np.zeros((6,));\n",
    "Q = puma.rne(puma.qn, zero, zero)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b6f1704c",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = puma.rne(puma.qn, zero, zero, gravity=[0, 0, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c7d7457",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj = jtraj(puma.qz, puma.qr, 10);\n",
    "Q = puma.rne(traj.q, traj.qd, traj.qdd);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9890cc1c",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "509ba2f5",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q[5, :]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5606bbda",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.rne(puma.qn, [1, 0, 0, 0, 0, 0], zero, gravity=[0, 0, 0])\n",
    "\n",
    "print(puma[1].dyn())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5291f38a",
   "metadata": {},
   "source": [
    "## 9.2.1 Gravity Term\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78e445aa",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = puma.gravload(puma.qn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "491eb19d",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.gravity"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d96219e8",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.gravity /= 6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cf796d6c",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.gravload(puma.qn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0c9fe46f",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.base = SE3.Rx(pi);\n",
    "puma.gravload(puma.qn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13aa4c51",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4d78ed66",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = puma.gravload(puma.qs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2027975",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q = puma.gravload(puma.qr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9d3a70ff",
   "metadata": {},
   "outputs": [],
   "source": [
    "N = 100;\n",
    "Q1, Q2 = np.meshgrid(np.linspace(-pi, pi, N), np.linspace(-pi, pi, N));\n",
    "G1, G2 = np.zeros((N,N)), np.zeros((N,N));\n",
    "for i in range(N):\n",
    "  for j in range(N):\n",
    "    g = puma.gravload(np.array([0, Q1[i,j], Q2[i,j], 0, 0, 0]))\n",
    "    G1[i, j] = g[1]  # shoulder gravity load\n",
    "    G2[i, j] = g[2]  # elbow gravity load\n",
    "plt.axes(projection=\"3d\").plot_surface(Q1, Q2, G1);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1d587f77",
   "metadata": {},
   "source": [
    "## 9.2.2 Inertia Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4137003e",
   "metadata": {},
   "outputs": [],
   "source": [
    "M = puma.inertia(puma.qn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad5fcf1f",
   "metadata": {},
   "outputs": [],
   "source": [
    "N = 100;\n",
    "Q1, Q2 = np.meshgrid(np.linspace(-pi, pi, N), np.linspace(-pi, pi, N));\n",
    "M00, M01, M11 = np.zeros((N,N)), np.zeros((N,N)), np.zeros((N,N));\n",
    "for i in range(N):\n",
    "  for j in range(N):\n",
    "    M = puma.inertia(np.array([0, Q1[i,j], Q2[i,j], 0, 0, 0]))\n",
    "    M00[i, j] = M[0, 0]\n",
    "    M01[i, j] = M[0, 1]\n",
    "    M11[i, j] = M[1, 1]\n",
    "plt.axes(projection=\"3d\").plot_surface(Q1, Q2, M00);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7caa88c",
   "metadata": {},
   "outputs": [],
   "source": [
    "M00.max() / M00.min()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b9d5c64d",
   "metadata": {},
   "source": [
    "## 9.2.3 Friction\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6f884cdd",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.friction([1, 0, 0, 0, 0, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "803edf27",
   "metadata": {},
   "source": [
    "## 9.2.4 Coriolis and Centripetal Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "91efae1a",
   "metadata": {},
   "outputs": [],
   "source": [
    "qd = [0, 0, 1, 0, 0, 0];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b50d896f",
   "metadata": {},
   "outputs": [],
   "source": [
    "C = puma.coriolis(puma.qn, qd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1b1bfc65",
   "metadata": {},
   "outputs": [],
   "source": [
    "C @ qd"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23630ec7",
   "metadata": {},
   "source": [
    "## 9.2.5 Effect of Payload\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5425ec9c",
   "metadata": {},
   "outputs": [],
   "source": [
    "G = puma.gravload(puma.qn);\n",
    "M = puma.inertia(puma.qn);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7fe383aa",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.payload(2.5, [0, 0, 0.1]);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ae2e47e",
   "metadata": {},
   "outputs": [],
   "source": [
    "M_loaded = puma.inertia(puma.qn);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "796587d7",
   "metadata": {},
   "outputs": [],
   "source": [
    "M_loaded / np.where(M < 1e-6, np.nan, M)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8d9cdc17",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.gravload(puma.qn) / np.where(G < 1e-6, np.nan, G)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1552194f",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.payload(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "26782863",
   "metadata": {},
   "source": [
    "## 9.2.6 Base Wrench\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ae1222a",
   "metadata": {},
   "outputs": [],
   "source": [
    "Q, wb = puma.rne(puma.qn, zero, zero, base_wrench=True);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e9ab98cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "wb"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0cefd5c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "sum([link.m for link in puma]) * puma.gravity[2]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ad1f6920",
   "metadata": {},
   "source": [
    "## 9.2.7 Dynamic Manipulability\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e7ae1e91",
   "metadata": {},
   "outputs": [],
   "source": [
    "Jt = puma.jacob0(puma.qn, half=\"trans\");  # first 3 rows"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "acd252d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "M = puma.inertia(puma.qn);\n",
    "E = (Jt @ np.linalg.inv(M) @ np.linalg.inv(M).T @ Jt.T);\n",
    "plot_ellipsoid(E);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f1def154",
   "metadata": {},
   "outputs": [],
   "source": [
    "e, _ = np.linalg.eig(E)\n",
    "radii = 1 / np.sqrt(e)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d41d8882",
   "metadata": {},
   "outputs": [],
   "source": [
    "radii.min() / radii.max()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f57348e6",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma.manipulability(puma.qn, method=\"asada\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "130aa717",
   "metadata": {},
   "source": [
    "# 9.3 Forward Dynamics\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "efaae59f",
   "metadata": {},
   "outputs": [],
   "source": [
    "qdd = puma.accel(puma.q, puma.qd, Q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e4e3828c",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m zerotorque -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"zerotorque\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c50d3b91",
   "metadata": {},
   "outputs": [],
   "source": [
    "xplot(out.t, out.x[:, :3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4be0686c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# note the next line is wrong in the book, there is an extra passed parameter 'robot'\n",
    "torque_func = lambda robot, t, q, qd: np.zeros((6,))\n",
    "traj = puma.nofriction().fdyn(T=5, q0=puma.qr, Q=torque_func)\n",
    "xplot(traj.t, traj.q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c8b37ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma_nf = puma.nofriction();"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "2cddd75e",
   "metadata": {},
   "source": [
    "# 9.4 Rigid-Body Dynamics Compensation\n",
    "#"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "554abb26",
   "metadata": {},
   "source": [
    "## 9.4.1 Feedforward Control\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b9b53909",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m feedforward -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"bdrun feedforward\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f32b4e85",
   "metadata": {},
   "source": [
    "## 9.4.2 Computed-Torque Control\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "382d0128",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m computed-torque-main -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"computed-torque-main\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fe254c4e",
   "metadata": {},
   "source": [
    "# 9.5 Task-Space Dynamics and Control\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d0ef384",
   "metadata": {},
   "outputs": [],
   "source": [
    "puma = models.DH.Puma560();\n",
    "\n",
    "xd = [0, 0.1, 0, 0, 0, 0];\n",
    "qd = np.linalg.inv(puma.jacob0_analytical(puma.qn, \"eul\")) @ xd;\n",
    "Cx = puma.coriolis_x(puma.qn, qd, representation=\"eul\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f807b43e",
   "metadata": {},
   "outputs": [],
   "source": [
    "Cx @ xd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "167aeba8",
   "metadata": {},
   "outputs": [],
   "source": [
    "Mx = puma.inertia_x(puma.qn, representation=\"eul\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c0ea616d",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.inv(Mx) @ [10, 0, 0, 0, 0, 0]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2210d94b",
   "metadata": {},
   "source": [
    "# 9.6 Applications\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "08924a7c",
   "metadata": {},
   "source": [
    "## 9.6.1 Operational Space Control\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fef5a257",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m opspace -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"opspace\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e8e8462",
   "metadata": {},
   "source": [
    "## 9.6.2 Series-Elastic Actuator (SEA)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f56c5329",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB:\n",
    "  %run -m SEA -H -g  # no graphics\n",
    "else:\n",
    "  run_shell(\"SEA\")"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "019dbbf6354d31390a1af2a8707908a74d3d4774daf62f20d47153cfbd4b4bc4"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
